#include <stdio.h>

#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/clocks.h"
#include "hardware/pll.h"
#include "hardware/clocks.h"
#include "hardware/vreg.h"

#include "hal.h"

int clock_init()
{
#define CPU_SPEED_MHZ (266000 / 1000)

if(CPU_SPEED_MHZ > 266 && CPU_SPEED_MHZ <= 360)
    vreg_set_voltage(VREG_VOLTAGE_1_20);
else if (CPU_SPEED_MHZ > 360 && CPU_SPEED_MHZ <= 396)
    vreg_set_voltage(VREG_VOLTAGE_1_25);
else if (CPU_SPEED_MHZ > 396)
    vreg_set_voltage(VREG_VOLTAGE_MAX);
else
    vreg_set_voltage(VREG_VOLTAGE_DEFAULT);

set_sys_clock_khz(CPU_SPEED_MHZ * 1000, true);
clock_configure(clk_peri,
                0,
                CLOCKS_CLK_PERI_CTRL_AUXSRC_VALUE_CLK_SYS,
                CPU_SPEED_MHZ * MHZ,
                CPU_SPEED_MHZ * MHZ); 
	return 0;
}

void measure_freqs(void) {
    volatile uint f_pll_sys = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_PLL_SYS_CLKSRC_PRIMARY);
    volatile uint f_pll_usb = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_PLL_USB_CLKSRC_PRIMARY);
    volatile uint f_rosc = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_ROSC_CLKSRC);
    volatile uint f_clk_sys = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_CLK_SYS);
    volatile uint f_clk_peri = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_CLK_PERI);
    volatile uint f_clk_usb = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_CLK_USB);
    volatile uint f_clk_adc = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_CLK_ADC);
    volatile uint f_clk_rtc = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_CLK_ADC);

    printf("pll_sys  = %dkHz\n", f_pll_sys);
    printf("pll_usb  = %dkHz\n", f_pll_usb);
    printf("rosc     = %dkHz\n", f_rosc);
    printf("clk_sys  = %dkHz\n", f_clk_sys);
    printf("clk_peri = %dkHz\n", f_clk_peri);
    printf("clk_usb  = %dkHz\n", f_clk_usb);
    printf("clk_adc  = %dkHz\n", f_clk_adc);
    printf("clk_rtc  = %dkHz\n", f_clk_rtc);

    // Can't measure clk_ref / xosc as it is the ref
}

void HAL::Init()
{
    clock_init();
    
    stdio_init_all();

    gpio_init(PICO_DEFAULT_LED_PIN);
    gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);

    measure_freqs();
    /*
    Serial.begin(115200);

    // Move the malloc process to Init() to make sure that the largest heap can be used for this buffer.
    disp_draw_buf = static_cast<lv_color_t*>(malloc(DISP_BUF_SIZE * sizeof(lv_color_t)));
    if (disp_draw_buf == nullptr)
        Serial.printf("lv_port_disp_init malloc failed!\n");
    power_init();
    motor_init();

    knob_init();
    // super_dial_init();
    */
}


void HAL::Update()
{
    //__IntervalExecute(HAL::knob_update(), 10);
    
    // HAL::TaskMotorUpdate(NULL);
}
